#include "cutrace.h"
#include <math.h>
#include <stdio.h>

extern double Bfr[Nx*Ny];
extern double Bfz[Nx*Ny];
extern double Bfphi[Nx*Ny];
extern double Apsi[Nx*Ny];

/*lagrange interpolation*/
double interp(double r, double z, double field[Nx*Ny])
{
    int rg = (int)((r - Rmin)/dr);
    int zg = (int)((z - Zmin)/dz);
    double ofr = (r - (double)rg * dr - Rmin)/dr;
    double ofz = (z - (double)zg * dz - Zmin)/dz;
    double ret = 0.0;
    double coefrs[4] = {ofr*(ofr-1.0)*(ofr-2.0)/-6.0, (ofr+1.0)*(ofr-1.0)*(ofr-2.0)/2.0,
    (ofr+1.0)*ofr*(ofr-2.0)/-2.0, (ofr+1.0)*ofr*(ofr-1.0)/6.0};
    double coefzs[4] = {ofz*(ofz-1.0)*(ofz-2.0)/-6.0, (ofz+1.0)*(ofz-1.0)*(ofz-2.0)/2.0,
    (ofz+1.0)*ofz*(ofz-2.0)/-2.0, (ofz+1.0)*ofz*(ofz-1.0)/6.0};
    for(int i = 0; i < 4; i++)
        for(int j = 0; j < 4; j++)
        {
            ret += coefrs[i]*coefzs[j]*field[Ny*(rg+i-1)+zg+j-1];
        }
    return ret;
}

bool checkboundary(vec vp)
{
    double pr = sqrt(vp.x*vp.x + vp.y*vp.y);
    double pz = vp.z;
    if (pr > Rmax || pr < Rmin)
        return false;
    if (pz > Zmax || pz < Zmin)
        return false;
    
    double psi = interp(pr, pz, Apsi);
    if (psi == 0.0)
        return false;
    return true;
}

vec vectorfield(vec vp)
{
    double pr, pphi;
    pr = sqrt(vp.x*vp.x+vp.y*vp.y);
    pphi = atan(vp.y/vp.x) + pi*(vp.x<0.0) + 2*pi*(vp.y < 0.0 && vp.x > 0.0);
    double Br = interp(pr, vp.z, Bfr);
    double Bphi = interp(pr, vp.z, Bfphi);
    vec vB = {Br*cos(pphi)-Bphi*sin(pphi),Br*sin(pphi)+Bphi*cos(pphi),interp(pr, vp.z, Bfz)};
    return vB;
}

void diagnose(vec vp, vec vv, char mode)
{
    vec vB = vectorfield(vp);
    vec gdc = vp + 1.0/qmr/dot(vB,vB)*cross(vv, vB);
    vB = vectorfield(gdc);
    double pR = sqrt(gdc.x*gdc.x+gdc.y*gdc.y);
    double pphi = atan(gdc.y/gdc.x) + pi*(gdc.x<0.0) + 2*pi*(gdc.y < 0.0 && gdc.x > 0.0);
    double ppsi = interp(pR,gdc.z,Apsi);
    double vpara2 = pow(dot(vB, vv),2)/dot(vB, vB);
    double vperp2 = dot(vv, vv) - vpara2;
    double Pphi = -ppsi*qmr + sqrt(vpara2)*pR;
    double mu = 0.5*vperp2/sqrt(dot(vB,vB));
    
    switch (mode)
    {
    case 'd':
        printf("diagnosis: R:%lf, phi:%lf, Z:%lf, psi:%lf, vpara2:%lf, vperp2:%lf, Pphi:%lf, mu:%lf\n",
    pR, pphi*360.0/(2*pi), gdc.z, ppsi, vpara2, vperp2, Pphi, mu);
        break;
    case 'c':
        printf("diagnosis: Pphi:%lf, mu:%lf, E:%lf\n", Pphi, mu, 0.5*(vperp2+vpara2));
        break;
    case 'p':
        printf("diagnosis: ");
        break;
    default:
        break;
    }
    return;
}